#include <ros/ros.h>
#include <std_msgs/Float64.h>

namespace inv_pen
{
    class SituPub
    {
    public:
        double get_rate(void);
        void loop(void);
        SituPub(std::string ns);

    private:
        double situ_vel;
        double angle_vel;
        double dt;
        double rate;
        std_msgs::Float64 situ;
        std_msgs::Float64 angle;
        std_msgs::Float64 u_feedback;
        ros::NodeHandle pnh;
        ros::Subscriber situ_vel_sub;
        ros::Subscriber angle_vel_sub;
        ros::Publisher situ_pub;
        ros::Publisher angle_pub;
        ros::Publisher u_feedback_pub;
        void SituVelCallBack(const std_msgs::Float64 &msg);
        void AngleVelCallBack(const std_msgs::Float64 &msg);
    };
}

inv_pen::SituPub::SituPub(std::string ns) : situ_vel(0.0), angle_vel(0.0), dt(1.00/35.0), rate(35)
{
    situ.data = 0.0;
    angle.data = 0.0;
    u_feedback.data = 0.0;
    pnh = ros::NodeHandle(ns);
    situ_vel_sub = pnh.subscribe("/situ_vel", 1, &inv_pen::SituPub::SituVelCallBack, this);
    angle_vel_sub = pnh.subscribe("/angle_vel", 1, &inv_pen::SituPub::AngleVelCallBack, this);
    situ_pub = pnh.advertise<std_msgs::Float64>("/situ", 2);
    angle_pub = pnh.advertise<std_msgs::Float64>("/angle", 2);
    u_feedback_pub = pnh.advertise<std_msgs::Float64>("/unsigned_feedback",2);
}

void inv_pen::SituPub::SituVelCallBack(const std_msgs::Float64 &msg)
{
    situ_vel = msg.data;
}

void inv_pen::SituPub::AngleVelCallBack(const std_msgs::Float64 &msg)
{
    angle_vel = msg.data;
}

double inv_pen::SituPub::get_rate(void)
{
    return rate;
}

void inv_pen::SituPub::loop(void)
{
    ros::spinOnce();
    /*
    接收从vel-pub传来的速度消息，积分得位置，并pub,并且把反馈给计算出来并发布
    unsigned表示不表明正负反馈的类型
    如果是负反馈，那就需要在接收这个话题消息的地方添加负号
    */
    situ.data += situ_vel * dt;
    angle.data += angle_vel * dt;
    situ_pub.publish(situ);
    // angle_pub.publish(angle);
    ROS_INFO("pub situ:%e\t%e", situ.data, angle.data);
    u_feedback.data = 35.4250*situ.data +16.0345*situ_vel +(-17.1603)*angle.data +0.6648*angle_vel;
    u_feedback_pub.publish(u_feedback);
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "situ_pub_node");
    inv_pen::SituPub situpub("");
    ros::Rate rate(situpub.get_rate());
    while (ros::ok())
    {
        situpub.loop();
        rate.sleep();
    }
    ros::shutdown();
    return 0;
}